#!/bin/bash

source /opt/ros/hydro/setup.bash
#export ROS_WORKSPACE=~/catkin_ws/src/revrev-vivo/
#export ROS_HOSTNAME=Dennisjcj
#export ROS_MASTER_URI=http://10.42.0.65:11311
cd ~/catkin_ws/src/revrev-vivo/
source devel/setup.bash

echo "Starting robot code"

echo "Launching Gimbals Controller"
cd ~/catkin_ws/src/revrev-vivo/src/dynamixel_manager/launch/
roslaunch gimbals_controller.launch&

sleep 5

echo "Gimbals Controller Launched"
	

read -p "Press Enter to start Dynamixel Manager"
echo "Launching Dynamixel Manager"
	
chmod a+rw /dev/ttyUSB0
roslaunch dynamixel_manager.launch&

sleep 8

echo "Dynamixel Manager Launched"

read -p "Press Enter to start Rosserial"
echo "Launching rosserial_startup.sh"

chmod a+rw /dev/ttyACM0
rosrun rosserial_python serial_node.py /dev/ttyACM0 _baud:=57600&

sleep 5
echo "Rosserial Launched"

echo "Starting controller code"

#echo "Launching Mother Ship"

#rosparam set max_speed_scale 30
#rosrun mother_ship mother_ship&
#echo "Mother Ship Set Sail"

read -p "Press Enter to clear screen and start to control"
printf "\ec"
#echo "Please Start Keyborad Control on another terminal"
rosrun keyboard_ctrl keyboard_ctrl
echo "Commander Stand By, please give command:"



